#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
import sys


class AltitudeSetter(Node):
    """简单的高度设置工具"""
    
    def __init__(self):
        super().__init__('altitude_setter')
        
        # 发布器
        self.altitude_pub = self.create_publisher(
            Float64,
            '/target_altitude',
            10
        )
        
    def set_altitude(self, altitude):
        """设置目标高度"""
        msg = Float64()
        msg.data = float(altitude)
        
        # 发布几次确保收到
        for _ in range(5):
            self.altitude_pub.publish(msg)
            rclpy.spin_once(self, timeout_sec=0.1)
            
        self.get_logger().info(f'目标高度已设置为: {altitude:.2f}m')


def main():
    if len(sys.argv) != 2:
        print("用法: ros2 run pb2025_sentry_nav set_altitude.py <高度值>")
        print("例如: ros2 run pb2025_sentry_nav set_altitude.py 0.8")
        return
        
    try:
        altitude = float(sys.argv[1])
    except ValueError:
        print("错误: 高度值必须是数字")
        return
        
    rclpy.init()
    
    setter = AltitudeSetter()
    setter.set_altitude(altitude)
    
    setter.destroy_node()
    rclpy.shutdown()
    
    print(f"高度目标已发送: {altitude:.2f}m")


if __name__ == '__main__':
    main() 